/** ###################################################################
**     Filename  : gps.c
**     Project   : gps
**     Processor : MC9S12XDT512BCFU
**     Version   : Driver 01.14
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 24.10.2011, 8:14
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/
/* MODULE gps */

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "AS1.h"
#include "GPIO1.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

/* User includes (#include below this line is not maintained by Processor Expert) */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>

#define GPS1_ID    3
#define GPS2_ID    4
#define GPS_ID     GPS2_ID

typedef struct 
{
 word id;
 byte len;
 byte data[8];
} can_message_t;

typedef struct 
{
  float tim;
  float lat;
  float lon;
  float alt;
  float speed;
  byte  val;
  byte  sat;
  float hdop;
  float vdop;
} gps_data_t;

byte xtoi( const char* xs, unsigned int* result);
byte gps_checksum( char *a_field);

byte can_recv( can_message_t *pmess);
byte can_send( can_message_t *pmess);
void can_init( void);

void gps_parse_gsa(void);
void gps_parse_gga(void);
void gps_parse_rmc(void);
void send_nans(void);
void send_data(void);

#define LED0 PORTA_PA7
#define LED1 PORTA_PA6
#define LED2 PORTA_PA5

#define CANE   0x80
#define INITAK 0x01
#define INITRQ 0x01
#define SLPRQ  0x02
#define SLPAK  0x02

byte gps_prot = 0;
byte gps_line_index = 0;
char gps_line[200] = "";
//byte gps_refreshed = 0;

gps_data_t gps_data;

unsigned int Sending = 0;

// converts a hexadecimal string to integer
byte xtoi( const char* xs, unsigned int* result)
{
  size_t szlen = strlen( xs);
  int i, xv, fact;

  if( szlen > 0)
  {
    // converting more than 32bit hexadecimal value
    if( szlen > 8)
    {
      return 0;
    }

    // begin conversion here
    *result = 0;
    fact = 1;

    // run until no more character to convert
    for( i = szlen - 1; i >= 0 ; i--)
    {
      if( isxdigit( *( xs + i)))
      {
        if( *( xs + i) >= 97)
        {
          xv = ( *( xs+i) - 97) + 10;
        }
        else if ( *( xs + i) >= 65)
        {
          xv = (*( xs + i) - 65) + 10;
        }
        else
        {
          xv = *( xs + i) - 48;
        }
        *result += (xv * fact);
        fact *= 16;
      }
      else
      {
        return 0;
      }
    }
 }
 
 return 1;
}

byte gps_checksum( char *a_field)
{
  char sum = 0;
  word check;
    
  for( ; *a_field != '*' && *a_field != '\0'; a_field++)
  {
    sum ^= *a_field;
  }

  if(*a_field++ != '*')
    return 0;

  if( strlen( a_field) < 2)
    return 0;
  
  if( xtoi( a_field, &check) != 1)
    return 0;

  return ( ( ( unsigned char) sum == check) ? 1 : 0);
}

void gps_parse_gsa()
{
  /*
    $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
     GSA      Satellite status
     A        Auto selection of 2D or 3D fix (M = manual) 
     3        3D fix - values include: 1 = no fix
                                       2 = 2D fix
                                       3 = 3D fix
     04,05... PRNs of satellites used for fix (space for 12) 
     2.5      PDOP (dilution of precision) 
     1.3      Horizontal dilution of precision (HDOP) 
     2.1      Vertical dilution of precision (VDOP)
     *39      the checksum data, always begins with *
  */
  
  byte i;
  byte sec;
  byte len;
  char buf[20] = "";
  byte buf_index = 0;
  byte max_sec = 0;
  
  float hdop = 0;
  float vdop = 0;
  
  len = ( byte) strlen( gps_line);
  sec = 0;
  
  for( i = 0; i < len; i++)
  {
    if( gps_line[i] == ',' || gps_line[i] == '*')
    {
      max_sec++;
    }
  }
  
  for( i = 0; i < len; i++)
  {
    if( gps_line[i] == ',' || gps_line[i] == '*')
    {
      buf[buf_index] = 0;

      if( sec == max_sec - 2)
      {
        hdop = ( float) atof( &buf[0]);
      }
      
      if( sec == max_sec - 1)
      {
        vdop = ( float) atof( &buf[0]);
        break;
      }
      
      sec++;
      buf_index = 0;
      ( void) memset( &buf[0], 0, sizeof( buf));
      continue;
    }
    if( buf_index < sizeof( buf))
    {
      buf[buf_index] = gps_line[i];
      buf_index++;
    }
    else // section too long
    {
      break;
    }
  }
  
  gps_data.hdop = hdop;
  gps_data.vdop = vdop;
  
  //gps_refreshed |= 1;
  
  return;
}

void gps_parse_gga()
{
  /*
  Global Positioning System Fix Data. Time, position and fix related data for a GPS receiver. 

  $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
  1    = UTC of Position
  2    = Latitude
  3    = N or S
  4    = Longitude
  5    = E or W
  6    = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix)
  7    = Number of satellites in use [not those in view]
  8    = Horizontal dilution of position
  9    = Antenna altitude above/below mean sea level (geoid)
  10   = Meters  (Antenna height unit)
  11   = Geoidal separation (Diff. between WGS-84 earth ellipsoid and
         mean sea level.  -=geoid is below WGS-84 ellipsoid)
  12   = Meters  (Units of geoidal separation)
  13   = Age in seconds since last update from diff. reference station
  14   = Diff. reference station ID#
  15   = Checksum
  */
  
  double d = 0.0f;
  double m = 0.0f;
  byte i;
  byte sec;
  byte len;
  char buf[20] = "";
  byte buf_index = 0;
  
  float tim = 0;
  float lon = 0;
  float lat = 0;
  float alt = 0;
  byte val = 0;
  
  len = ( byte) strlen( gps_line);
  sec = 0;
  for( i = 0; i < len; i++)
  {
    if( gps_line[i] == ',')
    {
      buf[buf_index] = 0;
      switch( sec)
      {
      case 0: // gpgga
        break;
        
      case 1: // time
        tim = atof( &buf[0]);
        break;
        
      case 2: // latitude
        lat = atof( &buf[0]);
	      m = 100.0 * modf( lat / 100.0, &d);
	      lat = ( float) (d + m / 60.0);
        break;

      case 3: // latitude mark
        if( buf[0] == 'S')
        {
          lat *= -1.0f;
        }
        break;
        
      case 4: // longitude 
        lon = ( float) atof( &buf[0]);
	      m = 100.0 * modf( lon / 100.0, &d);
	      lon = ( float) ( d + m / 60.0);
        break;
        
      case 5: // longitude mark
        if( buf[0] == 'W')
        {
          lon *= -1.0f;
        }
        break;
        
      case 6: // validity
        if( buf[0] == '0')
        {
          val = 0;
        }
        else
        {
          val = 1;
        }
        break;
        
      case 7: // gps satellites fix
        break;
        
      case 8: // horizontal dilution of position
        break;
        
      case 9: // altitude
        alt = atof( &buf[0]);
        break;
        
      case 10: // altitude unit
        break;
        
      default:
        break;
      }
      sec++;
      buf_index = 0;
      //( void) memset( &buf[0], 0, sizeof( buf));
      continue;
    }
    if( buf_index < sizeof( buf))
    {
      buf[buf_index] = gps_line[i];
      buf_index++;
    }
    else // section too long
    {
      break;
    }
  }
  
  gps_data.tim = tim;
  gps_data.lat = lat;
  gps_data.lon = lon;
  gps_data.alt = alt;
  gps_data.val = val;
  
//  gps_refreshed |= 2;

  return;   
}

void gps_parse_rmc()
{
  /*
  Recommended minimum specific GPS/Transit data
  
  $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh
  1    = UTC of position fix
  2    = Data status (V=navigation receiver warning)
  3    = Latitude of fix
  4    = N or S
  5    = Longitude of fix
  6    = E or W
  7    = Speed over ground in knots
  8    = Track made good in degrees True
  9    = UT date
  10   = Magnetic variation degrees (Easterly var. subtracts from true course)
  11   = E or W
  12   = Checksum
  */
  
  double d = 0.0f;
  double m = 0.0f;
  byte i;
  byte sec;
  byte len;
  char buf[20] = "";
  byte buf_index = 0;
  
  float speed = 0;
//  float lon = 0;
//  float lat = 0;
  
  len = ( byte) strlen( gps_line);
  sec = 0;
  for( i = 0; i < len; i++)
  {
    if( gps_line[i] == ',')
    {
      buf[buf_index] = 0;
      switch( sec)
      {
      case 0: // gprmc
        break;
        
      case 1: // time
        break;
        
      case 2: // data status
        break;

      case 3: // latitude
/*        lat = atof( &buf[0]);
	      m = 100.0 * modf( lat / 100.0, &d);
	      lat = ( float) (d + m / 60.0);        */
        break;
        
      case 4: // latitude mark
 /*       if( buf[0] == 'S')
        {
          lat *= -1.0f;
        }                                   */
        break;
        
      case 5: // longitude
/*        lon = ( float) atof( &buf[0]);
	      m = 100.0 * modf( lon / 100.0, &d);
	      lon = ( float) ( d + m / 60.0);       */
        break;
        
      case 6: // longitude mark
/*        if( buf[0] == 'W')
        {
          lon *= -1.0f;
        }                      */
        break;
        
      case 7: // speed
        speed = atof( &buf[0]) * 0.514444f;
        break;
        
      case 8: // 
        break;
        
      case 9: // 
        break;
        
      case 10: //
        break;
      
      case 11: //
        break;
        
      default:
        break;
      }
      sec++;
      buf_index = 0;
      ( void) memset( &buf[0], 0, sizeof( buf));
      continue;
    }
    if( buf_index < sizeof( buf))
    {
      buf[buf_index] = gps_line[i];
      buf_index++;
    }
    else // section too long
    {
      break;
    }
  }
  
  gps_data.speed = speed;
//  gps_data.lat = lat;
//  gps_data.lon = lon;
  
  //gps_refreshed |= 4;
//  gps_refreshed = 7;
  
  return;
}

void main(void)
{
  /* Write your local variable definition here */
  int ret = 0; 
  can_message_t msg;
  byte c = 0;
  byte i = 0;
  
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  can_init();

  LED0 = 1;
  LED1 = 1;
 
  gps_prot = 0;
  
  for( ;;)
  {
    LED2 = ~LED2;
    ret = AS1_RecvChar( &c);

    if( ret == ERR_OK)
    {
      switch( gps_prot)
      {
        case 0: // waiting for sentense start
          if( c == '$')
          {
            gps_prot = 1;
            gps_line_index = 0;
          }
        break;
      
        case 1: // store received characters and check for end of sentence
          if( c == '\r' || c == '\n')
          {
            gps_line[gps_line_index] = 0;
            
            ret = gps_checksum( &gps_line[0]);
            if( ret == 1)
            {
              LED0 = ~LED0;

              // GGA sentence
              ret = memcmp( &gps_line[0], "GPGGA", 5);
      	      if( ret == 0)
              {
                gps_parse_gga();
              }             
              
              // RMC sentence
              ret = memcmp( &gps_line[0], "GPRMC", 5);
      	      if( ret == 0)
              {
                gps_parse_rmc();
              }
              
              // GSA sentence
              ret = memcmp( &gps_line[0], "GPGSA", 5);
      	      if( ret == 0)
              {
                gps_parse_gsa();
              }          
            }           
            
            ( void) memset( &gps_line, 0, sizeof( gps_line));

            gps_prot = 0;
          }
          else
          {
            gps_line[gps_line_index] = c;
            gps_line_index++;
            if( gps_line_index >= sizeof( gps_line))
            {
              ( void) memset( &gps_line, 0, sizeof( gps_line));
              gps_prot = 0;
            }
          }
        break;
      
        default:
          gps_prot = 0;
        break;
      }
    }
  
    ret = can_recv( &msg);
    if( ret == 1)
    {
      CAN0RFLG = 0x01;   
      if (msg.id == 1)  
      {
        if (msg.data[0] == 1)
        {
          
          Sending ++;
          if( Sending >= 2)
          {
            LED1 = ~LED1;
            Sending = 0;
            if(gps_data.hdop!=0  && gps_data.vdop!=0) {
              send_data();
           
            } else {
              send_nans(); 
            }
          }
          else
          {
            send_nans();
          }
        }
        else
        {
          send_nans();
        }
      }
    }
  }

  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/


void send_nans()
{
  can_message_t msg;
  
  msg.len = 4;
  msg.data[0] = GPS_ID;
  msg.data[1] = 2;
  msg.data[2] = 0;
  msg.data[3] = 0;
  
// gps_refreshed = 0;
  msg.id = 0x040c;
  while( !can_send( &msg));

  msg.id = 0x040d;
  while( !can_send( &msg));

  msg.id = 0x040e;
  while( !can_send( &msg)); 

  msg.id = 0x040f;
  while( !can_send( &msg));  

  msg.id = 0x0514;
  while( !can_send( &msg));

  msg.id = 0x0515;
  while( !can_send( &msg));
}

void send_data()
{
  can_message_t msg;
  
  msg.len = 8;
  msg.data[0] = GPS_ID;
  msg.data[1] = 2;
  msg.data[2] = 0;
  msg.data[3] = 0;
  
  msg.id = 0x040c;
  msg.data[4] = ( ( unsigned char*) &gps_data.lat)[0];
  msg.data[5] = ( ( unsigned char*) &gps_data.lat)[1];
  msg.data[6] = ( ( unsigned char*) &gps_data.lat)[2];
  msg.data[7] = ( ( unsigned char*) &gps_data.lat)[3];
  while( !can_send( &msg));

  msg.id = 0x040d;
  msg.data[4] = ( ( unsigned char*) &gps_data.lon)[0];
  msg.data[5] = ( ( unsigned char*) &gps_data.lon)[1];
  msg.data[6] = ( ( unsigned char*) &gps_data.lon)[2];
  msg.data[7] = ( ( unsigned char*) &gps_data.lon)[3];
  while( !can_send( &msg));

  msg.id = 0x040e;
  msg.data[4] = ( ( unsigned char*) &gps_data.alt)[0];
  msg.data[5] = ( ( unsigned char*) &gps_data.alt)[1];
  msg.data[6] = ( ( unsigned char*) &gps_data.alt)[2];
  msg.data[7] = ( ( unsigned char*) &gps_data.alt)[3];
  while( !can_send( &msg)); 

  msg.id = 0x040f;
  msg.data[4] = ( ( unsigned char*) &gps_data.speed)[0];
  msg.data[5] = ( ( unsigned char*) &gps_data.speed)[1];
  msg.data[6] = ( ( unsigned char*) &gps_data.speed)[2];
  msg.data[7] = ( ( unsigned char*) &gps_data.speed)[3];
  while( !can_send( &msg));  

  msg.id = 0x0514;
  msg.data[4] = ( ( unsigned char*) &gps_data.vdop)[0];
  msg.data[5] = ( ( unsigned char*) &gps_data.vdop)[1];
  msg.data[6] = ( ( unsigned char*) &gps_data.vdop)[2];
  msg.data[7] = ( ( unsigned char*) &gps_data.vdop)[3];
  while( !can_send( &msg));

  msg.id = 0x0515;
  msg.data[4] = ( ( unsigned char*) &gps_data.hdop)[0];
  msg.data[5] = ( ( unsigned char*) &gps_data.hdop)[1];
  msg.data[6] = ( ( unsigned char*) &gps_data.hdop)[2];
  msg.data[7] = ( ( unsigned char*) &gps_data.hdop)[3];
  while( !can_send( &msg));
}



byte can_recv( can_message_t *pmess)
{
 byte i, tmp;
  
 if( CAN0RFLG & 0x01) 
 {
   pmess->id = CAN0RXIDR0 << 8;
   pmess->id += CAN0RXIDR1;
   if( pmess->id & 0x0010) 
   { 
     pmess->len = 0x80;
   }
   else
   {
     pmess->len = 0;
   }
  
   pmess->id = ( pmess->id >> 5) & 0x07ff;
                                                                                                    
   if( ( CAN0RXDLR & 0x0f) > 8) 
   {
     pmess->len |= 0x08;
     tmp = 8;
   }
   else	
   {
     tmp = CAN0RXDLR & 0x0f;
     pmess->len |= tmp;  
   }
   
   for( i=0; i < tmp; i++)
   {
     pmess->data[i] = (&CAN0RXDSR0)[i];
   }
   return 1;
  }
  return 0;
}

byte can_send( can_message_t *pmess)
{
  char i;
  unsigned short volatile *pireg;
 
  if( CAN0TFLG & 0x01)
  {
    pireg = ( unsigned short volatile *)( &CAN0TXIDR0);
    if( pmess->len & 0x80)
    { 
      *pireg = ( ( pmess->id << 5) & 0xffe0) | 0x0010;	// write id, rtr frame
    }
    else
    {
      *pireg = ( ( pmess->id << 5) & 0xffe0); // write id, data frame
    }
    pireg++;
    *pireg = 0;
    
    pmess->len &= 0x0f;
    for( i = 0; i < pmess->len; i++)
    {
      ( &CAN0TXDSR0)[i] = pmess->data[i];	  // write data
    }
    CAN0TXDLR = pmess->len; 	        // write dlc
    CAN0TFLG = 1;	 					  // start transmission
    return 1;
  }

  return 0;
}

void can_init(void)
{
  CAN0CTL0 |= INITRQ; // set initrq, this will also set initak
  while( !( CAN0CTL1 & INITAK)); // wait for init mode to occur

  CAN0CTL1 = CANE; // set cane just in case this is the first time after reset

  CAN0BTR0=0x01; // for 500kbps 0x03, for 100kbps 0x13
  CAN0BTR1=0x32; // for a can Baud of 500kbps at 16Mhz crystal

  // can id filter bits
  CAN0IDAR0=0x00;
  CAN0IDAR1=0x00;
  CAN0IDAR2=0x00;
  CAN0IDAR3=0x00;
  CAN0IDAR4=0x00;
  CAN0IDAR5=0x00;
  CAN0IDAR6=0x00;
  CAN0IDAR7=0x00;

  // can id filter mask
  CAN0IDMR0=0xff;
  CAN0IDMR1=0xff;
  CAN0IDMR2=0xff;
  CAN0IDMR3=0xff;
  CAN0IDMR4=0xff;
  CAN0IDMR5=0xff;
  CAN0IDMR6=0xff;
  CAN0IDMR7=0xff;

  CAN0IDAC = 0x10; // set filters to 2 16 bit acceptance filters

  CAN0CTL0 &= ~INITRQ; // clear initrq
  while (CAN0CTL1 & INITAK);

  CAN0CTL1 |= CANE; // set cane just in case this is the first time after reset
  CAN0RIER_RXFIE = 0; // enable receive interrupt

  CAN0TBSEL = 1;
}

/* END gps */
/*
** ###################################################################
**
**     This file was created by Processor Expert 3.02 [04.44]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
